#include "module/rviz_visualization.h"

#include <QDebug>
#include <QException>

namespace project_template {

RvizVisualization::RvizVisualization(QVBoxLayout* layout) {
  // 创建rviz panel
  render_panel_ = new rviz::RenderPanel();
  // 向layout添加
  layout->addWidget(render_panel_);
  // 创建rviz控制对象
  manager_ = new rviz::VisualizationManager(render_panel_);
  tool_manager_ = manager_->getToolManager();
  ROS_ASSERT(manager_ != nullptr);
  // 初始化render_panel 实现放大缩小等操作
  render_panel_->initialize(manager_->getSceneManager(), manager_);
  manager_->setFixedFrame("map");
  // 初始化rviz控制对象
  manager_->initialize();
  manager_->startUpdate();
  manager_->removeAllDisplays();
}

void RvizVisualization::SetFixedFrame(QString Frame_name) {
  manager_->setFixedFrame(Frame_name);
  SetViewInit();
}

void RvizVisualization::SetViewInit() {
  rviz::ViewManager* viewManager = manager_->getViewManager();
  viewManager->setRenderPanel(render_panel_);
  viewManager->setCurrentViewControllerType("rviz/TopDownOrtho");
  viewManager->getCurrent()->subProp("Near Clip Distance")->setValue("0.01");
  viewManager->getCurrent()->subProp("Target Frame")->setValue("<Fixed_Frame>");
  viewManager->getCurrent()->subProp("Scale")->setValue("56.4247");
  viewManager->getCurrent()->subProp("Angle")->setValue("3.14501");
  viewManager->getCurrent()->subProp("X")->setValue("0");
  viewManager->getCurrent()->subProp("Y")->setValue("0");
}

void RvizVisualization::DisplayGrid(int cell_count, int cell_size, QColor color,
                            bool enable) {
  if (grid_ != nullptr) {
    delete grid_;
    grid_ = nullptr;
  }
  grid_ = manager_->createDisplay("rviz/Grid", "myGrid", enable);
  // 设置cell Count
  grid_->subProp("Plane Cell Count")->setValue(cell_count);
  grid_->subProp("Cell Size")->setValue(cell_size);
  // 设置颜色
  grid_->subProp("Color")->setValue(color);
  ROS_ASSERT(grid_ != nullptr);
}

void RvizVisualization::DisplayAxes(bool enable) {
  if (axes_ != nullptr) {
    delete axes_;
    axes_ = nullptr;
  }
  axes_ = manager_->createDisplay("rviz/Axes", "myAxes", enable);
  ROS_ASSERT(axes_ != nullptr);
}

void RvizVisualization::DisplayMarker(bool enable) {
    if (marker_ != nullptr) {
        delete marker_;
        marker_ = nullptr;
    }
    marker_ = manager_->createDisplay("rviz/Marker", "marker", enable);
    marker_->subProp("Marker Topic")->setValue("/marker_name"); // 将话题名更改为需要显示的话题
    marker_->subProp("Queue Size")->setValue(1);
    ROS_ASSERT(marker_ != nullptr);
}

void RvizVisualization::DisplayMarkerArray(bool enable) {
  if (marker_array_ != nullptr) {
    delete marker_array_;
    marker_array_ = nullptr;
  }
  marker_array_ =
      manager_->createDisplay("rviz/MarkerArray", "markerarray", enable);
  marker_array_->subProp("Marker Topic")->setValue("/markerarray_name"); // 将话题名更改为需要显示的话题
  marker_array_->subProp("Queue Size")->setValue(100);
  ROS_ASSERT(marker_array_ != nullptr);
}

 void RvizVisualization::DisplayLaserCloud(bool enable) {
  if (laser_cloud_ != nullptr) {
    delete laser_cloud_;
    laser_cloud_ = nullptr;
  }

  laser_cloud_ =
      manager_->createDisplay("rviz/PointCloud2", "pointcloud2", enable);
  laser_cloud_->subProp("Topic")->setValue("/laser_cloud_name"); // 将话题名更改为需要显示的话题
  laser_cloud_->subProp("Style")->setValue("Flat Squares");
  laser_cloud_->subProp("Size (m)")->setValue(0.02);
  laser_cloud_->subProp("Alpha")->setValue(1);
  laser_cloud_->subProp("Decay Time")->setValue(0);
  laser_cloud_->subProp("Color Transformer")->setValue("AxisColor");
  laser_cloud_->subProp("Channel Name")->setValue("Intensity");
  laser_cloud_->subProp("Queue Size")->setValue(10);
  ROS_ASSERT(laser_cloud_ != nullptr);
}

}  // namespace project_template
